#include <iostream>
#include <string>
#include <fstream>
#include <thread>
#include <deque>
#include <yaml-cpp/yaml.h>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <GeographicLib/LocalCartesian.hpp>

class saveOdom2TUM
{
public:
    std::string root_dir = ROOT_DIR;
    std::string Leg_Odom_Topic;
    std::string SLAM_Odom_Topic;
    std::string GPS_Topic;

    std::string Leg_Odom_Path;
    std::string SLAM_Odom_Path;
    std::string GPS_Path;

    std::string saveFile_LegOdom;
    std::string saveFile_SLAMOdom;
    std::string saveFile_GPS;

    int reflash_File_LegOdom;
    int reflash_File_SLAMOdom;

    ros::NodeHandle nh;
    ros::Subscriber subLegOdom;
    ros::Subscriber subSLAMOdom;
    ros::Subscriber subNavsatfix;

    std::ofstream Path2TUMFiles;




    saveOdom2TUM()
    {
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/saveOdom2TUMConfig.yaml");
        Leg_Odom_Topic  = yaml["Leg_Odom_Topic"].as<std::string>();     // 查询config的配置文件
        SLAM_Odom_Topic = yaml["SLAM_Odom_Topic"].as<std::string>();    // 查询config的配置文件
        GPS_Topic       = yaml["GPS_Topic"].as<std::string>();          // 查询config的配置文件

        Leg_Odom_Path   = yaml["Leg_Odom_Path"].as<std::string>();      // 查询config的配置文件
        SLAM_Odom_Path  = yaml["SLAM_Odom_Path"].as<std::string>();     // 查询config的配置文件
        GPS_Path        = yaml["GPS_Path"].as<std::string>();           // 查询config的配置文件

        saveFile_LegOdom  = std::string(std::string(ROOT_DIR)) + Leg_Odom_Path;
        saveFile_SLAMOdom = std::string(std::string(ROOT_DIR)) + SLAM_Odom_Path;
        saveFile_GPS      = std::string(std::string(ROOT_DIR)) + GPS_Path;
        
        reflash_File_LegOdom = system((std::string("exec rm ") + saveFile_LegOdom).c_str());
        reflash_File_SLAMOdom = system((std::string("exec rm ") + saveFile_SLAMOdom).c_str());

        subLegOdom = nh.subscribe<nav_msgs::Odometry>(Leg_Odom_Topic, 1000, &saveOdom2TUM::LegOdom_handle, this, ros::TransportHints().tcpNoDelay());
        subSLAMOdom = nh.subscribe<nav_msgs::Odometry>(SLAM_Odom_Topic, 1000, &saveOdom2TUM::SLAMOdom_handle, this, ros::TransportHints().tcpNoDelay());

    }

    void LegOdom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;
        
        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(saveFile_LegOdom, time, x, y, z, qx, qy, qz, qw);
    }

    void SLAMOdom_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        nav_msgs::Odometry data = *msgIn;

        double time, x, y, z, qx, qy, qz, qw;
        time = data.header.stamp.toSec();
        x = data.pose.pose.position.x;
        y = data.pose.pose.position.y;
        z = data.pose.pose.position.z;
        qx = data.pose.pose.orientation.x;
        qy = data.pose.pose.orientation.y;
        qz = data.pose.pose.orientation.z;
        qw = data.pose.pose.orientation.w;

        saveData2TUM(saveFile_SLAMOdom, time, x, y, z, qx, qy, qz, qw);
    }

    /************************************************************
     * @brief 将数据写入成TUM的轨迹数据格式文件
     * 
     * @param file 文件路径
     * @param time 时间戳
     * @param x 
     * @param y 
     * @param z 
     * @param qx 
     * @param qy 
     * @param qz 
     * @param qw 
     ***********************************************************/
    void saveData2TUM(std::string file, double time, double x, double y, double z, double qx, double qy, double qz, double qw)
    {
        Path2TUMFiles.open(file, std::ios::app);
        if (Path2TUMFiles.is_open())
        {
            Path2TUMFiles << std::fixed << time
                << " " << x
                << " " << y
                << " " << z
                << " " << qx
                << " " << qy
                << " " << qz
                << " " << qw
                << std::endl;
            Path2TUMFiles.close();
        }
    }

    void saveOdomProcess()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            rate.sleep();
        }    
    }

};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "saveOdom2TUM");

    saveOdom2TUM MO;

    ROS_INFO("\033[1;32m----> saveOdom2TUM Procee Started.\033[0m");

    std::thread recordPath2TUMThread(&saveOdom2TUM::saveOdomProcess, &MO);

    ros::spin();

    recordPath2TUMThread.join();

    return 0;
}